#include "TimeIntegrator.h"

Vec<double,6> TimeIntegrator :: Newton(const Vec<double,6> &x,const Function &f,const JacobiFunction &ja,const double betas,const Vec<double,6> &res,double eps){
    Vec<double,6> next_x(x);
    int count = 1;
    while(maxnorm(f(next_x) * k * betas - next_x + res) > eps){
        next_x = nextx(next_x,f,ja,betas,res);
        count++;
        if(count > 1000){
            //std::cout << "Exceed!" << std::endl;
            break;
        }
    }
    return next_x;
}
    
Vec<double,6> TimeIntegrator :: nextx(const Vec<double,6> &x,const Function &f,const JacobiFunction &ja,const double betas,const Vec<double,6> &res){
    double J[36];
    double fun[6];
    Vec<double,6> next_x,fx = f(x);
    Vec<double,36> jx = ja(x);
    for(int i = 0;i < 6;i++){
        for(int j = 0;j<6;j++)
            J[i*6+j] = jx[i*6+j] * k * betas;
        J[i*7] = J[i*7] - 1;
        fun[i] = -(fx[i] * k * betas - x[i] + res[i]);
    }
    int ipiv[6];
    int info = LAPACKE_dgesv(LAPACK_COL_MAJOR,6,1,J,6,ipiv,fun,6);
    if(info == 0)
        for(int j = 0;j < 6;j++)
            next_x[j] = x[j] + fun[j];
    return next_x;
}
    
double TimeIntegrator :: maxnorm(const Vec<double,6> &x){
    double M = abs(x[0]);
    for(int i = 1;i < 6;i++){
        if(abs(x[i]) > M) M = abs(x[i]);
    }
    return M;
}

Vec<double,6> AdamsBashforth :: Implement(const Vec<double,6> &u0,const Function &f,const JacobiFunction &ja,int p,bool ISOUT,std::string filename){
    std::ofstream out(filename);
    double t = 0;
    Vec<double,6> un(u0);
    std::vector<Vec<double,6>> fu;
    double beta[4][4] = {1,0,0,0,3.0/2,-1.0/2,0,0,23.0/12,-16.0/12,5.0/12,0,55.0/24,-59.0/24,37.0/24,-9.0/24};
    fu.insert(fu.begin(),f(un));
    if(ISOUT) out << std::setw(20) << un[0] << std::setw(20) << un[1] << std::endl;
    for(int i = 1;i < p;i++){
        un = un + fu[0] * k;
        fu.insert(fu.begin(),f(un));
        t = t + k;
        if(ISOUT) out << std::setw(20) << un[0] << std::setw(20) << un[1] << std::endl;
    }
    while(t <= T){
        for(int i = 0;i < p;i++)
            un = un + fu[i] * beta[p-1][i] * k;
        fu.insert(fu.begin(),f(un));
        fu.pop_back();
        t = t + k;
        if(ISOUT) out << std::setw(20) << un[0] << std::setw(20) << un[1] << std::endl;
    }
    out.close();
    return un;
}

Vec<double,6> AdamsMoulton :: Implement(const Vec<double,6> &u0,const Function &f,const JacobiFunction &ja,int p,bool ISOUT,std::string filename){
    std::ofstream out(filename);
    double t = 0;
    Vec<double,6> un(u0),res;
    std::vector<Vec<double,6>> fu;
    double beta[4][5] = {1.0/2,1.0/2,0,0,0,5.0/12,8.0/12,-1.0/12,0,0,9.0/24,19.0/24,-5.0/24,1.0/24,0,251.0/720,646.0/720,-264.0/720,106.0/720,-19.0/720};
    fu.insert(fu.begin(),f(un));
    if(ISOUT) out << std::setw(20) << un[0] << std::setw(20) << un[1] << std::endl;
    for(int i = 1;i < p-1;i++){
        un = un + fu[0] * k;
        fu.insert(fu.begin(),f(un));
        t = t + k;
        if(ISOUT) out << std::setw(20) << un[0] << std::setw(20) << un[1] << std::endl;
    }
    while(t <= T){
        res = un;
        for(int i = 0;i < p-1;i++)
            res = res + fu[i] * beta[p-2][i+1] * k;
        un = Newton(un,f,ja,beta[p-2][0],res,1e-10);
        fu.insert(fu.begin(),f(un));
        fu.pop_back();
        t = t + k;
        if(ISOUT) out << std::setw(20) << un[0] << std::setw(20) << un[1] << std::endl;
    }
    out.close();
    return un;
}

Vec<double,6> BDF :: Implement(const Vec<double,6> &u0,const Function &f,const JacobiFunction &ja,int p,bool ISOUT,std::string filename){
    std::ofstream out(filename);
    double t = 0;
    Vec<double,6> un(u0),res;
    std::vector<Vec<double,6>> u;
    double beta[4] = {1,2.0/3,6.0/11,12.0/25};
    double alpha[4][4] = {-1,0,0,0,-4.0/3,1.0/3,0,0,-18.0/11,9.0/11,-2.0/11,0,-48.0/25,36.0/25,-16.0/25,3.0/25};
    u.insert(u.begin(),un);
    if(ISOUT) out << std::setw(20) << un[0] << std::setw(20) << un[1] << std::endl;
    for(int i = 1;i < p;i++){
        un = un + f(un) * k;
        u.insert(u.begin(),un);
        t = t + k;
        if(ISOUT) out << std::setw(20) << un[0] << std::setw(20) << un[1] << std::endl;
    }
    while(t <= T){
        res = u[0] * alpha[p-1][0] * (-1);
        for(int i = 1;i < p;i++)
            res = res - u[i] * alpha[p-1][i];
        un = Newton(un,f,ja,beta[p-1],res,1e-10);
        u.insert(u.begin(),un);
        u.pop_back();
        t = t + k;
        if(ISOUT) out << std::setw(20) << un[0] << std::setw(20) << un[1] << std::endl;
    }
    out.close();
    return un;
}

Vec<double,6> RungeKutta :: Implement(const Vec<double,6> &u0,const Function &f,const JacobiFunction &ja,int p,bool ISOUT,std::string filename){
    std::ofstream out(filename);
    double t = 0;
    Vec<double,6> y1,y2,y3,y4;
    Vec<double,6> un(u0);
    if(ISOUT) out << std::setw(20) << un[0] << std::setw(20) << un[1] << std::endl;
    while(t <= T){
        y1 = f(un);
        y2 = f(un + y1 * k/2);
        y3 = f(un + y2 * k/2);
        y4 = f(un + y3 * k);
        un = un + (y1 + y2 * 2 + y3 * 2 + y4) * k/6;
        t = t + k;
        if(ISOUT) out << std::setw(20) << un[0] << std::setw(20) << un[1] << std::endl;
    }
    out.close();
    return un;
}
